
#include "comm.h"
#include "configuration.h"
#include <math.h>

struct EncoderInput
{
	int left, right;
};

struct EncoderResult
{
	double x, y, angle;
};


const int PACKET_LENGTH = 5;

int extract_component(char low, char high)
{
	int ret = 0;
	if ( high & 0x80 ) ret = -1;
	ret |= (high << 8);
	ret |= low;
	return ret;
}

DWORD WINAPI ISR(LPVOID parent_object)
{
	EncoderDevice * encoder = (EncoderDevice*) parent_object;
	EncoderInput information;
	std::string chunk;
	while (1)
	{
		chunk += encoder->port.read(1);
		while ( chunk.size() >= PACKET_LENGTH )
		{
			int left=0, right=0;
			if (chunk[0] == '@') {
				if(REVERTED_ENCODERS) {
					right = extract_component(chunk[1], chunk[2]);
					left  = extract_component(chunk[3], chunk[4]);
				}
				else {
					left  = extract_component(chunk[1], chunk[2]);
					right = extract_component(chunk[3], chunk[4]);
				}
				information.left = left;
				information.right = right;
				encoder->process_information(information);
				chunk.erase(chunk.begin(), chunk.begin() + PACKET_LENGTH);
			}
			else {
				chunk.erase(chunk.begin());
			}
		}
	}
}

class EncoderDevice
{
	CommPort port;
	HANDLE	listening_thread;
	DWORD	thread_id;

	// position
	double		pos_x, pos_y, angle;

	// processing queue missing ..

	friend DWORD WINAPI ISR(LPVOID parent_object);
public:

	EncoderDevice(int portnumber, int baudrate) :
		port(portnumber, baudrate)
	{
		listening_thread = CreateThread(NULL, 0, &ISR, this, 0, &thread_id);
		if( listening_thread == NULL )
		{
			throw "could not create thread";
		}

	}

	void reset(double x, double y, double angle_)
	{
		// synchronization logic missing ..
	}
	EncoderResult read()
	{
		// synchronization logic missing ..
		EncoderResult ret;
		ret.x = pos_x;
		ret.y = pos_y;
		ret.angle = angle;
		return ret;
	}



private:
	void process_information(EncoderInput & data)
	{
		// save data to processing queue
		// synchronization logic missing ..
	}

	void position_calculation(int left_, int right_)
	{
		if ( left == 0 && right == 0 ) return;
		double left = (double) left_ * SIGNAL_TO_DISTANCE;
		double right = (double) right_ * SIGNAL_TO_DISTANCE;
		
		if (left == right) {
			pos_x += left * cos(angle);
			pos_y += left * sin(angle);
		}
		if (left != right) {
			pos_x += (AXIAL_DISTANCE * (right+left)) / (2.0 * (right-left)) * (sin((right-left)/AXIAL_DISTANCE+angle) - sin(angle));
			pos_y -= (AXIAL_DISTANCE * (right+left)) / (2.0*(right-left)) * (cos((right-left)/AXIAL_DISTANCE+angle) - cos(angle));
			angle += (right-left) / AXIAL_DISTANCE;
		}
		if (angle >= pi2) angle -= pi2;
		if (angle < 0.0) angle += pi2;
	}
	
};

